% cascade IIR filter stages

[bb,aa]=cheby2(5,60,0.3); 
[sos,g]=tf2sos(bb,aa);

%sos
%   1.0000000   1.0000000           0   1.0000000  -0.6207074           0
%   1.0000000  -0.2838348   1.0000000   1.0000000  -1.3517908   0.4972300
%   1.0000000  -1.1079438   1.0000000   1.0000000  -1.6343371   0.7919217

b1_1=sos(1,2);
a1_1=sos(1,5);
b1_0=(1+a1_1)/(1+b1_1); % stage 1 scale factor

b2_1=sos(2,2);
b2_2=sos(2,3);
a2_1=sos(2,5);
a2_2=sos(2,6);
b2_0=(1+a2_1+a2_2)/(1+b2_1+b2_2);  % stage 2 scale factor

b3_1=sos(3,2);
b3_2=sos(3,3);
a3_1=sos(3,5);
a3_2=sos(3,6);
b3_0=(1+a3_1+a3_2)/(1+b3_1+b3_2);  % stage 3 scale factor

w1_1=0;
w2_1=0;
w2_2=0;
w3_1=0;
w3_2=0;

x0=[1 zeros(1,100)];

y1=zeros(1,100);

for n=1:100
    w1_0=x0(n)-w1_1*a1_1;               % feedback term
    x1=(w1_0+w1_1*b1_1)*b1_0;           % scaled feed forward term
    w1_1=w1_0;                          % shift feedback term w1_0 into reg w1_1

    w2_0=x1-w2_1*a2_1-w2_2*a2_2;        % feedback terms
    x2=(w2_0+w2_1*b2_1+w2_2*b2_2)*b2_0; % scaled feed forward terms
    w2_2=w2_1;                          % shift reg w2_1 nto reg w2_2
    w2_1=w2_0;                          % shift feedback term w2_0 into reg w2_1
    
    w3_0=x2-w3_1*a3_1-w3_2*a3_2;        % feedback terms
    x3=(w3_0+w3_1*b3_1+w3_2*b3_2)*b3_0; % scaled feed forward terms
    w3_2=w3_1;                          % shift reg w3_1 nto reg w3_2
    w3_1=w3_0;                          % shift feedback term w3_0 into reg w3_1
    
    y1(n)=x3;
    
end

figure(1)
subplot(2,1,1)
plot(y1)
grid on
axis([0 100 -0.05 0.20])
title('Filter Impulse response')

subplot(2,1,2)
plot(-0.5:1/1024:0.5-1/1024,fftshift(20*log10(abs(fft(y1,1024)))))
grid on
axis([0 0.5 -80 10])
title('Frequency Response')


x0=zeros(1,200);
x0=x0+cos(2*pi*(0:199)*0.05 +2*pi*rand(1));
x0=x0+cos(2*pi*(0:199)*0.15 +2*pi*rand(1));
x0=x0+cos(2*pi*(0:199)*0.25 +2*pi*rand(1));
x0=x0+cos(2*pi*(0:199)*0.35 +2*pi*rand(1));


y2=zeros(1,200);

for n=1:200
    w1_0=x0(n)-w1_1*a1_1;               % feedback term
    x1=(w1_0+w1_1*b1_1)*b1_0;           % scaled feed forward term
    w1_1=w1_0;                          % shift feedback term w1_0 into reg w1_1

    w2_0=x1-w2_1*a2_1-w2_2*a2_2;        % feedback terms
    x2=(w2_0+w2_1*b2_1+w2_2*b2_2)*b2_0; % scaled feed forward terms
    w2_2=w2_1;                          % shift reg w2_1 nto reg w2_2
    w2_1=w2_0;                          % shift feedback term w2_0 into reg w2_1
    
    w3_0=x2-w3_1*a3_1-w3_2*a3_2;        % feedback terms
    x3=(w3_0+w3_1*b3_1+w3_2*b3_2)*b3_0; % scaled feed forward terms
    w3_2=w3_1;                          % shift reg w3_1 nto reg w3_2
    w3_1=w3_0;                          % shift feedback term w3_0 into reg w3_1
    
    y2(n)=x3;
    
end

figure(2)
subplot(2,2,1)
plot(x0/2)
grid on
axis([0 100 -1.5 1.5])
title('Input Sequence')

subplot(2,2,2)
plot(y2)
grid on
axis([0 100 -1.2 1.2])
title('Filtered Sequence')

ww=kaiser(200,8)';
ww=ww/sum(ww);
subplot(2,2,3)
plot(-0.5:1/1024:0.5-1/1024,fftshift(20*log10(abs(fft(x0.*ww,1024)))))
grid on
axis([0 0.5 -80 10])
title('Input Spectrum')


subplot(2,2,4)
plot(-0.5:1/1024:0.5-1/1024,fftshift(20*log10(abs(fft(y2.*ww,1024)))))
grid on
axis([0 0.5 -80 10])
title('Output Spectrum')

